#include "dji_linux_helpers.hpp"
#include "dji_linker.hpp"
#include "GSLAM/core/GSLAM.h"

using namespace GSLAM;
using namespace sv;

int main(int argc,char** argv){}

REGISTER_SVAR_MODULE(osdk){
    Class<LinuxSetup>("LinuxSetup")
            .def("__init__",[](bool enableAdvancedSensing){
        std::shared_ptr<LinuxSetup> ret=std::make_shared<LinuxSetup>(0,nullptr,enableAdvancedSensing);
        Vehicle*   vehicle = ret->getVehicle();
        const char *acm_dev = ret->getEnvironment()->getDeviceAcm().c_str();
        vehicle->advancedSensing->setAcmDevicePath(acm_dev);

        return ret;
    })
            .def("getVehicle",&LinuxSetup::getVehicle);

    Class<Vehicle>("Vehicle")
            .def_readonly("linker",&Vehicle::linker)
            .def_readonly("legacyLinker",&Vehicle::legacyLinker)
            .def_readonly("subscribe",&Vehicle::subscribe)
            .def_readonly("broadcast",&Vehicle::broadcast)
            .def_readonly("control",&Vehicle::control)
            .def_readonly("camera",&Vehicle::camera)
            .def_readonly("gimbal",&Vehicle::gimbal)
            .def_readonly("mfio",&Vehicle::mfio)
            .def_readonly("mobileDevice",&Vehicle::mobileDevice)
            .def_readonly("missionManager",&Vehicle::missionManager)
            .def_readonly("hardSync",&Vehicle::hardSync)
            .def_readonly("cameraManager",&Vehicle::cameraManager)
            .def_readonly("flightController",&Vehicle::flightController)
            .def_readonly("gimbalManager",&Vehicle::gimbalManager)
            .def_readonly("advancedSensing",&Vehicle::advancedSensing)
            .def_readonly("mopServer",&Vehicle::mopServer)
            .def_readonly("djiBattery",&Vehicle::djiBattery)
            .def("activate",[](Vehicle* self,Vehicle::ActivateData* data, int timeoutMs){
        self->activate(data,timeoutMs);
    });

    Class<CameraManager>("CameraManager")
            .def("setModeSync",&CameraManager::setModeSync)
            .def("obtainDownloadRightSync",&CameraManager::obtainDownloadRightSync)
            .def("startReqFileList",[](CameraManager* self,PayloadIndexType p_index,Svar callback){

        return (int)self->startReqFileList(p_index,[](E_OsdkStat ret_code, const FilePackage file_list, void* userData){
            Svar cbk=*(Svar*)userData;
            cbk((int)ret_code,file_list);
            delete (Svar*)userData;
        },new Svar(callback));
    })
    .def("startReqFileData",[](CameraManager* self,PayloadIndexType p_index,int fileIndex,std::string localPath,Svar callback){

        return (int)self->startReqFileData(p_index,fileIndex,localPath,[](E_OsdkStat ret_code, void* userData){
            Svar cbk=*(Svar*)userData;
            cbk((int)ret_code);
            delete (Svar*)userData;
        },new Svar(callback));
    })
    .def("reqFileList",[](CameraManager* self,PayloadIndexType p_index){
        std::promise<Svar> ret;

        self->startReqFileList(p_index,[](E_OsdkStat ret_code, const FilePackage file_list, void* userData){
            std::promise<Svar>& ret=*(std::promise<Svar>*)userData;
            ret.set_value(Svar(file_list));
        },&ret);

        auto f=ret.get_future();
        f.wait();

        return f.get();
    })
    .def("reqFileData",[](CameraManager* self,PayloadIndexType p_index,int fileIndex,std::string localPath){
        std::promise<int> ret;

        (int)self->startReqFileData(p_index,fileIndex,localPath,[](E_OsdkStat ret_code, void* userData){
            std::promise<int>& ret=*(std::promise<int>*)userData;
            ret.set_value((int)ret_code);
        },&ret);

        auto f=ret.get_future();
        f.wait();

        return f.get();
    })
    ;

    Class<FilePackage>("FilePackage")
            .def_readonly("media",&FilePackage::media);

    Class<MediaFile>("MediaFile")
            .def_readonly("valid",&MediaFile::valid)
            .def_readonly("fileIndex",&MediaFile::fileIndex)
            .def_readonly("fileName",&MediaFile::fileName)
            .def_readonly("valid",&MediaFile::valid)
            .def("__repr__",[](MediaFile* self){
        return sv::Svar({{"fileIndex",self->fileIndex},
                         {"fileName",self->fileName},
                         {"fileSize",self->fileSize}}).dump_json();
    });

    Class<MobileDevice>("MobileDevice")
            .def("sendDataToMSDK",[](MobileDevice* self,SvarBuffer buf){
        return self->sendDataToMSDK((uint8_t*)buf.ptr(),buf.size());
    })
    ;

    Class<MopServer>("MopServer")
            .unique_construct<>()
            .def("accept",[](MopServer* self,int id)->MopPipeline*{
        MopPipeline* p=nullptr;
        if(self->accept(id,RELIABLE,p)==MOP_PASSED) return p;

        return nullptr;
    })
            .def("close",[](MopServer* self,int id){
        return self->close(id);
    });

    Class<MopPipeline>("MopPipeline")
            .def("sendData",[](MopPipeline* self,SvarBuffer buf){
        uint32_t sent=0;
        auto ret=self->sendData({(uint8_t*)buf.ptr(),buf.length()},&sent);
        if(ret!=MOP_PASSED)
            LOG(ERROR)<<"MopPipeline::sendData failed with code "<<ret;
        return (int)sent;
    })
    .def("recvData",[](MopPipeline* self,SvarBuffer buf){
        uint32_t received=0;
        auto ret=self->recvData({(uint8_t*)buf.ptr(),buf.length()},&received);
        if(ret!=MOP_PASSED)
            LOG(ERROR)<<"MopPipeline::sendData failed with code "<<ret;
        return received;
    });

    Class<AdvancedSensing>("AdvancedSensing")
            .def("changeH264Source",&AdvancedSensing::changeH264Source)
            .def("startFPVCameraStream",[](AdvancedSensing* self,Svar callback){
        self->startFPVCameraStream([](CameraRGBImage pImg, void* userData){
            Svar cbk=*(Svar*)userData;
            LOG(INFO) << "Received "<<pImg.height<<"X"<<pImg.width;
            cbk(GImage(pImg.height,pImg.width,GImageType<uchar,3>::Type,pImg.rawData.data(),true));
        },new Svar(callback));
    })
    .def("startMainCameraStream",[](AdvancedSensing* self,Svar callback){
        self->startMainCameraStream([](CameraRGBImage pImg, void* userData){
            Svar cbk=*(Svar*)userData;
            cbk(GImage(pImg.height,pImg.width,GImageType<uchar,3>::Type,pImg.rawData.data(),true));
        },new Svar(callback));
    })
    .def("startH264Stream",[](AdvancedSensing* self,LiveView::LiveViewCameraPosition pos,Svar callback){
        self->startH264Stream(pos,[](uint8_t* buf, int bufLen, void* userData){
            Svar cbk=*(Svar*)userData;
            cbk(SvarBuffer(buf,bufLen).clone());
        },new Svar(callback));
    });

    Class<DataBroadcast>("DataBroadcast")
            .def("setBroadcastFreqDefaults",[](DataBroadcast* self){
        return self->setBroadcastFreqDefaults();
    })
            .def("getTimeStamp",[](DataBroadcast* self)->Svar{
        auto time=self->getTimeStamp();
        return time.time_ms*0.001+time.time_ns*1e-9;
    })
    .def("getQuaternion",[](DataBroadcast* self){
        auto q=self->getQuaternion();
        return std::vector<double>({q.q1,q.q2,q.q3,q.q0});
    })
    .def("getVelocity",[](DataBroadcast* self){
        auto q=self->getVelocity();
        return std::vector<double>({q.x,q.y,q.z});
    })
    .def("getAngularRate",[](DataBroadcast* self){
        auto q=self->getAngularRate();
        return std::vector<double>({q.x,q.y,q.z});
    })
    .def("getGlobalPosition",[](DataBroadcast* self)->Svar{
        auto q=self->getGlobalPosition();
        static double rad2degree=180/3.1415926;
        return {{"health",(int)q.health},
            {"height",(double)q.height},
            {"altitude",(double)q.altitude},
            {"longitude",(double)q.longitude*rad2degree},
            {"latitude",(double)q.latitude*rad2degree}};
    })
    .def("getRelativePosition",[](DataBroadcast* self)->Svar{
        auto q=self->getRelativePosition();
        return {{"up",(double)q.up},
            {"back",(double)q.back},
            {"down",(double)q.down},
            {"front",(double)q.front},
            {"left",(double)q.left},
            {"right",(double)q.right},
            {"upHealth",(bool)q.upHealth},
            {"backHealth",(bool)q.backHealth},
            {"downHealth",(bool)q.downHealth},
            {"frontHealth",(bool)q.frontHealth},
            {"leftHealth",(bool)q.leftHealth},
            {"rightHealth",(bool)q.rightHealth}};
    })
    .def("getGimbal",[](DataBroadcast* self)->Svar{
        auto q=self->getGimbal();
        return {{"pitch",(double)q.pitch},
            {"yaw",(double)q.yaw},
            {"roll",(double)q.roll},
            {"pitchLimit",(bool)q.pitchLimit},
            {"yawLimit",(bool)q.yawLimit},
            {"rollLimit",(bool)q.rollLimit}};
    })
    .def("getBatteryInfo",[](DataBroadcast* self)->Svar{
        auto q=self->getBatteryInfo();
        return {{"current",(double)q.current},
            {"voltage",(double)q.voltage},
            {"capacity",(double)q.capacity},
            {"percentage",(double)q.percentage}};
    })
    ;


    Class<GimbalManager>("GimbalManager")
            .def("initGimbalModule",&GimbalManager::initGimbalModule)
            .def("getGimbalModuleName",&GimbalManager::getGimbalModuleName)
            .def("rotateSync",[](GimbalManager* self,PayloadIndexType index,sv::Svar target,int timeout){
         GimbalModule::Rotation rotation;
         rotation.pitch=target.get<double>("pitch",0);
         rotation.yaw=target.get<double>("yaw",0);
         rotation.roll=target.get<double>("roll",0);
         rotation.time=target.get<double>("time",0);
         return self->rotateSync(index,rotation,timeout);
    })
            .def("rotateAsync",[](GimbalManager* self,PayloadIndexType index,sv::Svar target,int timeout){
        GimbalModule::Rotation rotation;
        rotation.pitch=target.get<double>("pitch",0);
        rotation.yaw=target.get<double>("yaw",0);
        rotation.roll=target.get<double>("roll",0);
        rotation.time=target.get<double>("time",0);
        return self->rotateAsync(index,rotation,[](ErrorCode::ErrorCodeType retCode,UserData userData){
            LOG(INFO)<<"rotation ret:"<<retCode;
        },nullptr);
   });


    Class<MissionManager>("MissionManager")
            .def("init",[](MissionManager* self){
        self->init(WAYPOINT);
    });
//            .def("");

    Class<int64_t>("int64_t")
            .construct<int>()
            .def("__int__",[](int64_t self){return (int)self;});

    svar["OSDK_CAMERA_POSITION_FPV"] = LiveView::OSDK_CAMERA_POSITION_FPV;
    svar["OSDK_CAMERA_POSITION_NO_1"] = LiveView::OSDK_CAMERA_POSITION_NO_1;
    svar["OSDK_CAMERA_POSITION_NO_2"] = LiveView::OSDK_CAMERA_POSITION_NO_2;
    svar["OSDK_CAMERA_POSITION_NO_3"] = LiveView::OSDK_CAMERA_POSITION_NO_3;

    svar["OSDK_CAMERA_SOURCE_DEF"] = LiveView::OSDK_CAMERA_SOURCE_DEF;
    svar["OSDK_CAMERA_SOURCE_H20_WIDE"] = LiveView::OSDK_CAMERA_SOURCE_H20_WIDE;
    svar["OSDK_CAMERA_SOURCE_H20T_WIDE"] = LiveView::OSDK_CAMERA_SOURCE_H20T_WIDE;
    svar["OSDK_CAMERA_SOURCE_H20_ZOOM"] = LiveView::OSDK_CAMERA_SOURCE_H20_ZOOM;
    svar["OSDK_CAMERA_SOURCE_H20T_ZOOM"] = LiveView::OSDK_CAMERA_SOURCE_H20T_ZOOM;
    svar["OSDK_CAMERA_SOURCE_H20T_IR"] = LiveView::OSDK_CAMERA_SOURCE_H20T_IR;

    svar["PAYLOAD_INDEX_0"] = PAYLOAD_INDEX_0;
    svar["PAYLOAD_INDEX_1"] = PAYLOAD_INDEX_1;
    svar["PAYLOAD_INDEX_2"] = PAYLOAD_INDEX_2;
    svar["PAYLOAD_INDEX_CNT"] = PAYLOAD_INDEX_CNT;
    svar["PAYLOAD_INDEX_INVALID"] = PAYLOAD_INDEX_INVALID;

    svar["SHOOT_PHOTO"] = CameraModule::WorkMode::SHOOT_PHOTO;
    svar["RECORD_VIDEO"] = CameraModule::WorkMode::RECORD_VIDEO;
    svar["PLAYBACK"] = CameraModule::WorkMode::PLAYBACK;
    svar["MEDIA_DOWNLOAD"] = CameraModule::WorkMode::MEDIA_DOWNLOAD;
    svar["BROADCAST"] = CameraModule::WorkMode::BROADCAST;
    svar["WORK_MODE_UNKNOWN"] = CameraModule::WorkMode::WORK_MODE_UNKNOWN;

    svar["printErrorCodeMsg"] = ErrorCode::printErrorCodeMsg;

    Class<ErrorCode>("ErrorCode")
            .def_static("printErrorCodeMsg",&ErrorCode::printErrorCodeMsg)
            ;

    Class<GImage>("GImage")
            .construct<>()
            .construct<GImage>()
            .construct<int,int,int>()
            .def("empty",&GImage::empty)
            .def_readonly("data",&GImage::data)
            .def("elemSize",&GImage::elemSize)
            .def("elemSize1",&GImage::elemSize1)
            .def("channels",&GImage::channels)
            .def("type",&GImage::type)
            .def("total",&GImage::total)
            .def("clone",&GImage::clone)
            .def("row",&GImage::row)
            .def("width",&GImage::getWidth)
            .def("height",&GImage::getHeight)
            .def("reshape",[](GImage self,int width,int height){
        assert(self.total()==width*height);
        self.cols=width;
        self.rows=height;
        return self;
    })
    .def("__repr__",[](const GImage& img){
        return to_string(img.cols)+"x"
                +to_string(img.rows)+"x"+to_string(img.channels());})
            .def("__init__",[](SvarBuffer buffer){
        if(buffer._holder.is<GImage>())
            return buffer._holder.as<GImage>();
        std::string format=buffer._format;
        std::vector<ssize_t> shape=buffer.shape;
        int channels=1;
        if(shape.size()==2)
            channels=1;
        else if(shape.size()==3)
            channels=shape[2];
        else
            LOG(FATAL)<<"Shape should be 2 or 3 demisson";

        int rows=shape[0];
        int cols=shape[1];

        static int lut[256]={0};
        lut['b']=0;lut['B']=1;
        lut['h']=2;lut['H']=3;
        lut['i']=4;lut['I']=5;
        lut['f']=6;lut['d']=7;
        int type=lut[format.front()]+((channels-1)<<3);
        return GImage(rows,cols,type,(uchar*)buffer.ptr(),buffer._holder);
    })
    .def("__buffer__",[](GImage& self){
        std::string formats[]={"b","B","h","H","i","I","f","d"};
        std::string format   =formats[(self.type()&0x7)];
        return SvarBuffer(self.data,self.elemSize1(),format,{self.rows,self.cols,self.channels()},{},self.holder);
    })
    ;

    Class<Messenger>("Messenger")
            .construct<>()
            .def_static("instance",&Messenger::instance)
            .def("getPublishers",&Messenger::getPublishers)
            .def("getSubscribers",&Messenger::getSubscribers)
            .def("introduction",&Messenger::introduction)
            .def("advertise",[](Messenger msg,const std::string& topic,int queue_size){
        return msg.advertise<sv::Svar>(topic,queue_size);
    })
    .def("subscribe",[](Messenger msger,
         const std::string& topic, int queue_size,
         const SvarFunction& callback){
        return msger.subscribe(topic,queue_size,callback);
    })
    .def("publish",[](Messenger* msger,std::string topic,sv::Svar msg){return msger->publish(topic,msg);});

    Class<Publisher>("Publisher")
            .def("shutdown",&Publisher::shutdown)
            .def("getTopic",&Publisher::getTopic)
            .def("getTypeName",&Publisher::getTypeName)
            .def("getNumSubscribers",&Publisher::getNumSubscribers)
            .def("publish",[](Publisher* pubptr,sv::Svar msg){return pubptr->publish(msg);});

    Class<Subscriber>("Subscriber")
            .def("shutdown",&Subscriber::shutdown)
            .def("getTopic",&Subscriber::getTopic)
            .def("getTypeName",&Subscriber::getTypeName)
            .def("getNumPublishers",&Subscriber::getNumPublishers);

    Class<SO3>("SO3")
                .construct<>()
                .construct<double,double,double,double>()
                .construct<const Point3d&,double>()
                .construct<const double*>()
                .def("log",&SO3::log)
                .def_static("exp",&SO3::exp<double>)
                .def_static("fromPitchYawRollAngle",&SO3::fromPitchYawRollAngle)
                .def("normalise",&SO3::normalise)
                .def("getMatrix",(Matrix<double,3,3> (SO3::*)()const)&SO3::getMatrix)
                .def("__mul__",&SO3::mul)
                .def("trans",&SO3::trans)
                .def("inverse",&SO3::inverse)
                .def("__repr__",&SO3::toString)
                .def_readwrite("x", &SO3::x)
                .def_readwrite("y", &SO3::y)
                .def_readwrite("z", &SO3::z)
                .def_readwrite("w", &SO3::w)
                ;

    Class<Point3d>("Point3d")
            .construct<>()
            .construct<double,double,double>()
            .def("dot",&Point3d::dot)
            .def("norm",&Point3d::norm)
            .def("at",&Point3d::at)
            .def("__repr__",&Point3d::toString)
            .def("__str__",&Point3d::toString)
            .def("__add__",&Point3d::add)
            .def("__sub__",&Point3d::sub)
            .def("__mul__",&Point3d::mul)
            .def("__div__",&Point3d::div)
            .def_readwrite("x", &Point3d::x)
            .def_readwrite("y", &Point3d::y)
            .def_readwrite("z", &Point3d::z);

    svar["messenger"]=Messenger::instance();

}

EXPORT_SVAR_INSTANCE
